#include "os.h"

static Task taskList[MAX_TASKS];
static unsigned int currentTaskId = 0;
static unsigned int nextTaskId = 0;
static unsigned int sysTicks = 0;
static unsigned int taskCount = 0;

Task *g_CurrentTask;
Task *g_NextTask;

void os_Init(void)
{

    for (int i = 0; i < MAX_TASKS; i++)
    {
        taskList[i].sp = 0;
        taskList[i].entry = 0;
        taskList[i].priority = PRIORITY_LOW;
        taskList[i].delay = 0;
        taskList[i].stackSize = 0;
        taskList[i].state = TASK_READY;
    }

    currentTaskId = 0;
    nextTaskId = 0;
    sysTicks = 0;
    taskCount = 0;
}

unsigned int os_TaskCreate(void (*entry)(void), char *name, TaskPriority priority, unsigned int stackSize) // 创建任务
{
    if (taskCount >= MAX_TASKS)
    {
        return 0xFF;
    }

    unsigned int taskId = taskCount++;
    Task *task = &taskList[taskId];
    task->name = name;
    task->entry = entry;
    task->priority = priority;
    task->delay = 0;
    task->stackSize = stackSize > STACK_SIZE ? STACK_SIZE : stackSize;
		// uint32_t *sp = (void *)((uint32_t)&task->stack + stackSize - 64);
    task->sp = task->stack + 1;
		task->stack[0] = (uint32_t)entry & 0xFF;
		task->stack[1] = (uint32_t)entry >> 8;
    task->state = TASK_READY;
	

    return taskId;
}

void os_TaskDelete(unsigned int taskId)
{
    if (taskId > taskCount)
    {
        return;
    }

    Task *task = &taskList[taskId];

    if (taskId < taskCount)
    {
        for (int i = taskId; i < taskCount; i++)
        {
            taskList[i] = taskList[i + 1];
        }
    }

    taskCount--;

    // 如果当前任务是要删除的任务，则切换到下一个任务
    if (taskId == currentTaskId)
    {
        os_ContextSwitch();
    }
}

void os_TaskSuspend(unsigned int taskId)
{
    if (taskId >= taskCount)
    {
        return;
    }

    taskList[taskId].state = TASK_SUSPENDED;

    // 如果当前任务是要挂起的任务，则切换到下一个任务
    if (taskId == currentTaskId)
    {
        os_ContextSwitch();
    }
}

void os_TaskResume(unsigned int taskId)
{
    if (taskId >= taskCount)
    {
        return;
    }

    if (taskList[taskId].state == TASK_SUSPENDED)
    {
        taskList[taskId].state = TASK_READY;
    }
}

void os_TaskDelay(unsigned int ms)
{
    if (currentTaskId >= taskCount)
    {
        return;
    }

    taskList[currentTaskId].delay = ms;
    taskList[currentTaskId].state = TASK_BLOCKED;

    os_ContextSwitch();
}

static unsigned int os_FindNextTask(void)
{
    if (taskCount == 0)
    {
        return 0xFF;
    }

    unsigned int next = (currentTaskId + 1) % taskCount;
    unsigned int i = next;
    TaskPriority h_Priority = PRIORITY_LOW;
    do
    {
        Task *nextTask = &taskList[i];
        if (nextTask->state == TASK_READY && nextTask->priority > h_Priority)
        {
            h_Priority = nextTask->priority;
            next = i;
        }
        i = (i + 1) % taskCount;
    } while (i != next);

    return next;
}

void os_ContextSwitch(void)
{
    nextTaskId = os_FindNextTask();
    if (nextTaskId == 0xFF || nextTaskId == currentTaskId)
    {
        return;
    }

    if (currentTaskId < taskCount && taskList[currentTaskId].state == TASK_RUNNING)
    {
        taskList[currentTaskId].state = TASK_READY;
    }

    taskList[nextTaskId].state = TASK_RUNNING;
    currentTaskId = nextTaskId;

    if (taskList[currentTaskId].entry != 0)
    {
        void (*taskFunc)(void) = taskList[currentTaskId].entry;
        taskFunc();
    }
}

void os_ScheduleTick(void)
{
    sysTicks++;

    // 检查任务是否需要延时
    for (int i = 0; i < taskCount; i++)
    {
        if (taskList[i].state == TASK_BLOCKED && taskList[i].delay > 0)
        {
            taskList[i].delay--;
            if (taskList[i].delay == 0)
            {
                taskList[i].state = TASK_READY;
            }
        }
    }

    // 触发任务调度
    os_ContextSwitch();
}

void os_ScheduleStart(void)
{
    if (taskCount == 0)
    {
        return;
    }

    currentTaskId = os_FindNextTask();
    if (currentTaskId != 0xFF)
    {
        taskList[currentTaskId].state = TASK_RUNNING;
    }
}

int os_GetSysTicks(void)
{
    return sysTicks;
}

int os_GetCurrentTaskId(void)
{
	return currentTaskId;
}